import math
import time
#import pygame
import sglibrary
from pyglet import clock
from sglibrary import Actor, Vector, Quaternion
#import display
try:
    import stackless
except ImportError:
    print "no stackless"
    raise

class SpaceShip(object):
    def __init__(self, input, position=(0,0,0),
                 rotation=Quaternion(1.,0.,0.,0.), mass=10000.0):
        self.mass = mass
	self.position = Vector(*position)
        self.velocity = 0
        self.acceleration = 0
        self.rotation = rotation
        self.input = input
        self.x_vals = {'alpha': 0, 'omega': 0}
        self.z_vals = {'alpha': 0, 'omega': 0}
	#stackless.tasklet(self._rotate_x)(xchannel)
	#stackless.tasklet(self._rotate_z)(zchannel)

    def _get_angle(self, force, vals, dt):
        if vals['omega'] > 0.05:
            vals['omega'] = 0.05
        if force == 0.0:
            vals['alpha'] = (-0.1 * vals['omega'])
        else:
            vals['alpha'] += (force / self.mass)
        vals['omega'] += vals['alpha'] * (dt)# * 1000)
        return vals['omega'] * (dt)# * 100)

    def _rotate_x(self, theta):
        #value, time_step = xchannel.receive()
        #theta =  angle / time_step
        self.rotation *= Quaternion(math.cos(theta/2), math.sin(theta/2), 0, 0)
	
    def _rotate_z(self, theta):#angle, time_step):
        #value, time_step = zchannel.receive()
        #theta = angle / time_step
        self.rotation *= Quaternion(math.cos(theta/2),0,0,math.sin(theta/2))#,0)

    def update(self):
        dt = clock.tick() * 30
        if dt:
            self.rotate(dt)
            self.apply_thrust(dt)

    def rotate(self, dt):
        z, x = self.input.axes
        angle_x = self._get_angle(x, self.x_vals, dt)
        self._rotate_x(angle_x)
        #self.rotation.normalise()
        angle_z = self._get_angle(z, self.z_vals, dt)
        self._rotate_z(angle_z)
        self.rotation.normalise()

    def apply_thrust(self, dt):
        self.acceleration = self.input.thrust / self.mass
        self.velocity += self.acceleration / dt
        self.position.z += self.velocity / dt
        #print self.position
        

##     def rotate(self, channel, _rotate, mass=10000.0):
## 	alpha = 0
## 	omega = 0
## 	#clock = pygame.time.Clock()
## 	#print clock.tick()
## 	while True:
##             #print "get force"
## 	    force = channel.receive()
##             #print "got"
## 	    if force == 0.0:
## 		alpha = -0.05 * omega
## 	    else:
## 		alpha += force / mass
## 	    dt = clock.tick()
##             if dt:
##                 omega += alpha / (dt * 1000)
##                 _rotate(omega / (dt * 1000))
##                 self.rotation.normalise()
##             stackless.schedule()

    def process_message(self, action, *args):
        #print args
        self.actions[action](*args)

#class SpaceShip(object):
    #rotx_vel = 0
    #rotx_acc = 0
    #last_xpoint = 0
    #anglex = 0
    #anglez = 0
    #clock = pygame.time.Clock()
    ##screen = display.Camera(self)

    #def __init__(self, xchannel, zchannel, position=(0,0,0)):
        #self.piover180 = math.pi/180
        #self.piover2 = math.pi/2
        #self.position = Vector(*position)
        #self.up = Vector(1, math.pi/4, 0)
        #self.target = Vector(-1, 0, 0)
        ##self.right = Vector(1, 0, math.pi/4)
        ##self.actions = {'turnx': self.rotatx, 'draw': self.draw}
        ##self.channel = stackless.channel()
        ##DEL#self.zchannel = stackless.channel()
        #stackless.tasklet(self.rotate)(xchannel, self._rotate_phi)
        #stackless.tasklet(self.rotate)(zchannel, self._rotate_theta)
        ##self.display = display.Camera(self)

    #def draw(self):
        #self.display.set_position()
        #self.display.draw()

    #def process_message(self, action, *args):
        ##print args
        #self.actions[action](*args)

    #def get_pos_or(self):
        #return (self.position.x, self.position.y, self.position.z,
                #self.target.x, self.target.y, self.target.z)

    #def _rotate_phi(self, angle):
        #"""Rotates in the x-y plane"""
	#self.up.y += angle * 0.1
	#self.target.y = angle * 0.1
        

    #def _rotate_theta(self, angle):
        #self.up.z += angle * 0.05
	#self.target.z = angle * 0.05

    #def polar2cartesian(self, vector):
	#x = vector.x * math.sin(vector.y) * math.cos(vector.z)
	#y = vector.x * math.sin(vector.y) * math.sin(vector.z)
	#z = vector.x * math.cos(vector.y)
	#return Vector(x, y, z)


    #def quat_rotate(self, angle):
        #pass

    #def rotate(self, channel, direction, mass=1000.0):
        #vel = 0
        #point = 0
        #angle = 0
        #clock = pygame.time.Clock()
        #pygame.time.delay(5)
        #while True:
            ### CHANGE THIS!! ##
            #force = channel.receive()
            #acceleration = force/mass
            #if acceleration == 0:
                #acceleration = 0.004 * -vel
            #old_vel = vel
            #elapsed = float(clock.tick())
            ##print elapsed
            #if elapsed:
                ##print (point - old_point) / (elapsed*100)
                #vel = old_vel + acceleration*elapsed
            ##print vel
            #direction(vel)

# class SpaceShip(object):
#     def __init__(self, position, channels):
#         self.position = Vector(position)
#         self.up = Vector((0,1,0))
#         self.target = Vector((0,0,-1))
#         self.right = Vector((1,0,0))
#         self.acceleration = 0
#         self.velocity = 0
#         self.angle = 0
#         self.piover180 = math.pi/180
#         self.piover2 = math.pi/2
#         self.channel = stackless.channel()
#         #self.move = stackless.tasklet(self.move)
#         #print self.move
#         self.rotatingx = False
#         self.rotatingz = False
#         stackless.tasklet(self.message_loop)()
#
#     def message_loop(self):
#         actions = {'turnx': self.rotatex, 'turny': self.rotatey,
#                    'stopx': self.stopx, 'stopy': self.stopy, 'move': self.move}
#         while 1:
#             action, args = self.channel.receive()
#             actions[action](args)
#
#
#
#
# #     def move(self):
# #         start_time = time.time()
# #         while 1:
# #             last_time = (self.channels['move'].receive() - start_time)
# #             if -0.1 <= self.velocity <= 0.1:
# #                 self.velocity += self.acceleration/last_time
# #             else:
# #                 self.velocity = self.velocity/self.velocity * -0.1
# #             #print 'v =', self.velocity
# #             self.position += self.target * (self.velocity * last_time)
#
#     def move(self):
#         self.position += self.target
#
#     def rotatex(self, rate):
#         start = time.time()
#         while self.rotatingx:
#             self._rotatex(rate/(time.time()-start))
#             start = time.time()
#             stackless.schedule()
#
#     def stopx(self, args):
#         self.rotatingx = False
#
#     def stopy(self, args):
#         self.rotatingy = False
#
#     def _rotatex(self, angle):
#         angle *= self.piover180
#         up = self.up[:]
#         target = self.target[:]
#         cosBeta  = math.cos(angle)
#         cosAlpha = math.cos(self.piover2 - angle)
#         self.up.x = cosBeta * up.x + cosAlpha * target.x
#         self.up[1] = cosBeta * up[1] + cosAlpha * target[1]
#         self.up.z = cosBeta * up.z + cosAlpha * target.z
#         cosAlpha = math.cos(self.piover2 + angle)
#         self.target.x = cosBeta * target.x + cosAlpha * up.x
#         self.target[1] = cosBeta * target[1] + cosAlpha * up[1]
#         self.target.z = cosBeta * target.z + cosAlpha * up.z
#
#     def rotatey(self, angle):
#         target = self.target[:]
#         right = self.right[:]
#         angle *= self.piover180
#         cosAlpha = math.cos(self.piover2 + angle)
#         cosBeta = math.cos(angle)
#         self.target.x = (cosBeta * target.x + cosAlpha * right.x)
#         self.target[1] = (cosBeta * target[1] + cosAlpha * right[1])
#         self.target[2] = (cosBeta * target[2] + cosAlpha * right[2])
#         cosAlpha = math.cos(self.piover2 - angle)
#         self.right.x = (cosAlpha * target.x + cosBeta * right.x)
#         self.right[1] = (cosAlpha * target[1] + cosBeta * right[1])
#         self.right[2] = (cosAlpha * target[2] + cosBeta * right[2])
#         #self.target.normalise()
#         #self.right.normalise()
#
#     def rotatez(self):
#         rotating = True
#         while rotating:
#             print 'rotate'
#             start = time.time()
#             rotating = self.channels['z'].receive()
#             self._rotatez(rate/(time.time()-start))
#
#     def _rotatez(self, angle):
#         up = self.up[:]
#         right = self.right[:]
#         angle *= self.piover180
#         cosAlpha = math.cos(self.piover2 + angle)
#         cosBeta = math.cos(angle)
#         self.right.x = cosAlpha * up.x + cosBeta * right.x
#         self.right[1] = cosAlpha * up[1] + cosBeta * right[1]
#         self.right[2] = cosAlpha * up[2] + cosBeta * right[2]
#         cosAlpha = math.cos(self.piover2 - angle)
#         self.up.x = cosBeta * up.x + cosAlpha * right.x
#         self.up[1] = cosBeta * up[1] + cosAlpha * right[1]
#         self.up[2] = cosBeta * up[2] + cosAlpha * right[2]
#
#     def check_collisions(self, objects):
#         collisions = []
#         for name, vector in objects:
#             if abs(self.position.x - vector.x) < 6 and \
#                abs(self.position.y - vector.y) < 6 and \
#                abs(self.position[2] - vector[2]) < 6:
#                 difference = position-vector
#                 if difference.magnitude_squared() < 36:
#                     collisions.append(name)
#         return collisions

class World(object):
    def __init__(self, size, objects):
        self.objects = objects

    def check_collisions(self):
        pass
